﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.IO;
using System.Media;
using System.Net;
using System.Net.Sockets;
using System.Text;
using System.Threading;
using System.Windows.Forms;
using com.atiia.automation.sensors;
using ControlSystem;

namespace SenRobCSTest
{
    public partial class MainForm : Form
    {
        VistaStyleProgressBar.ProgressBar p;

        Ethernet eth;
        Commands cmd;
        double speed = 20;
        int inc = 25;

        // trackpad
        bool trackpad_en = false;

        // movement vars
        string axis = "X";
        string movement = "Move";

        // robot selected
        int curRobot = 0; // 0: R1, 1: R2

        // joystick
        bool move = true;
        bool move1 = true;
        int selectedRobot = 1; // start with R1 selected

        // connection to ft sensors
        FTsensor ftSensors1;
        FTsensor ftSensors2;
        HapticControl RobotControl = new HapticControl();

        byte[] ftbuff = new byte[1024];
        Socket ftConnection;
        List<string> ftCmds = new List<string>();

        public MainForm()
        {
            InitializeComponent();

            eth = new Ethernet("146.232.146.111", 80);

            cmd = new Commands(eth);
            try
            {
                servoBox.Checked = ((int)cmd.RSTATS()[1] & 64) != 0;

                eth.getOutput().SendToBack();
                watchdog.Start();
            }
            catch (Exception e) {
                MessageBox.Show(e.StackTrace, "exception: " + e.Message);
                
            }

            PopulateFileList();

            // create and start both connections to the ft net boxes
            ftSensors1 = new com.atiia.automation.sensors.FTsensor("146.232.146.253", this, 1);
            ftSensors1.stopAndRestartSingleReadings();
            ftSensors2 = new com.atiia.automation.sensors.FTsensor("146.232.146.254", this, 2);
            ftSensors2.stopAndRestartSingleReadings();

            ftUpdate.Start();
        }

        public CheckBox getUDPtoggle() { return UDPtoggle; }

        public TextBox getFTBox() { return outputBox; }

        // not used
        private void receiveFTcomms()
        {
            initFTcomms();

            while (!Encoding.ASCII.GetString(ftbuff).Equals("exit"))
            {
                string msgIn = "";
                try
                {
                    ftConnection.Receive(ftbuff);
                    msgIn = Encoding.ASCII.GetString(ftbuff);
                    ftCmds.Add(msgIn);
                }
                catch (SocketException)
                {
                    MessageBox.Show("FT connection closed");
                }
            }
        }

        // not used
        private void initFTcomms()
        {
            Socket ftSock = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.IP);
            ftSock.Bind(new IPEndPoint(System.Net.IPAddress.Any, 9999));
            ftSock.Listen(0);
            ftConnection = ftSock.Accept();
        }

        // read command stream files available
        private void PopulateFileList()
        {
            string[] names = Directory.GetFiles(".", "*.csv");
            eth.print(names.Length.ToString());
            foreach (string s in names)
            {
                fileList.Items.Add(new ListViewItem(s));
            }
            fileList.Refresh();
        }

        // read commands file
        public void readCommandsFile(string filename)
        {
            try
            {
                // read all lines from file, then skip first line
                string[] readStringsHeader = System.IO.File.ReadAllLines(filename);
                string[] readStrings = new string[readStringsHeader.Length - 1];
                Array.Copy(readStringsHeader, 1, readStrings, 0, readStrings.Length);

                // update progressbar
                cmdProgress.MaxValue = readStrings.Length;
                cmdProgress.Value = 0;

                // populate command list
                cmdList.Items.Clear();
                foreach (string s in readStrings)
                {
                    cmdList.Items.Add(new ListViewItem(s));
                }
                cmdList.Refresh();

                cmdList.Items[0].Selected = true;
                cmdList.Refresh();

                int index = 0;

                foreach (string s in readStrings)
                {
                    cmdProgress.Value++;
                    if (inputString(s) != 1)
                        break;
                    cmdProgress.Refresh();
                    index++;
                }
            }
            catch (Exception e)
            {
                MessageBox.Show("Could not complete command stream.\n" + e.Message, "Error");
            }
        }

        System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();

        // perform one command from a string
        // see command file for format
        public int inputString(string command)
        {
            // don't need to read posision to ensure timeout does not occur
            watchdog_Tap();

            sw.Reset();
            sw.Start();

            string[] elements = command.Split(',');
            long before = sw.ElapsedMilliseconds;
            if (elements.Length >= 17)
            {
                string newCommand = "2,"; // create new command in correct format
                if (elements[0].Contains("IMOV"))
                {
                    newCommand = newCommand + "IMOV,";
                    // add velocity
                    newCommand = newCommand + elements[1] + ",";
                    // add x, y and z
                    newCommand = newCommand + elements[3] + "," + elements[4] + "," + elements[5] + ",";
                    // add angles and ending zeros
                    newCommand = newCommand + "0,0,0,0";
                    newCommand = newCommand.Replace(" ", "");
                    eth.print(newCommand);
                    elements = newCommand.Split(',');
                }
            }
            selectRobot(int.Parse(elements[0]));

            // handle input command
            if (elements[1].ToUpper().Equals("IMOV"))
            {
                before = sw.ElapsedMilliseconds;
                if (cmd.IMOV(double.Parse(elements[2]), double.Parse(elements[3]), double.Parse(elements[4]), double.Parse(elements[5]), double.Parse(elements[6]), double.Parse(elements[7]), double.Parse(elements[8]), double.Parse(elements[9])) != 0)
                {
                    //UDPtoggle.Checked = false;
                    outputBox.AppendText("Could not complete command sequence\n");
                    return 0;
                }
                //eth.print("move: (" + selectedRobot + ") " + (sw.ElapsedMilliseconds - before).ToString());
            }
            else if (elements[1].ToUpper().Equals("MOVJ"))
            {
                before = sw.ElapsedMilliseconds;
                if (cmd.MOVJ(double.Parse(elements[2]), double.Parse(elements[3]), double.Parse(elements[4]), double.Parse(elements[5]), double.Parse(elements[6]), double.Parse(elements[7]), double.Parse(elements[8]), double.Parse(elements[9])) != 0)
                {
                    MessageBox.Show("Could not complete command sequence");
                    return 0;
                }
                //eth.print("move: (" + selectedRobot + ") " + (sw.ElapsedMilliseconds - before).ToString());
            }
            else if (elements[1].ToUpper().Equals("HOME"))
                homeButton_Click(null, null);
            else if (elements[1].ToUpper().Equals("RESET"))
                resetButton_Click(null, null);
            else if (elements[1].ToUpper().Equals("SVON"))
                cmd.SVON(int.Parse(elements[2]) == 1);

            return 1;
        }

        // GUI button click functions
        private void status_Click(object sender, EventArgs e)
        {
            double[] read = cmd.RSTATS();
            eth.print("status: " + Convert.ToString((int)read[0], 2).PadLeft(8, '0') + " " + Convert.ToString((int)read[1], 2).PadLeft(8, '0'));
            servoBox.Checked = ((int)read[1] & 64) != 0;
        }

        private void servoBox_Click(object sender, EventArgs e)
        {
            cmd.SVON(servoBox.Checked);
        }

        private void Axis_SelectedIndexChanged(object sender, EventArgs e)
        {
            axis = Axis.Text;
        }

        private void movType_SelectedIndexChanged(object sender, EventArgs e)
        {
            movement = movType.Text;
        }

        // joystick buttons
        private void plusButton_Click(object sender, EventArgs e)
        {
            eth.print("axis: " + axis);
            eth.print("mov: " + movement);
            if (movement.Equals("Move"))
            {
                if (axis.Equals("X"))
                    cmd.IMOV(speed, inc, 0, 0, 0, 0, 0, 0);
                else if (axis.Equals("Y"))
                    cmd.IMOV(speed, 0, inc, 0, 0, 0, 0, 0);
                else
                    cmd.IMOV(speed, 0, 0, inc, 0, 0, 0, 0);
            }
            else
                if (axis.Equals("X"))
                    cmd.IMOV(speed, 0, 0, 0, inc, 0, 0, 0);
                else if (axis.Equals("Y"))
                    cmd.IMOV(speed, 0, 0, 0, 0, inc, 0, 0);
                else
                    cmd.IMOV(speed, 0, 0, 0, 0, 0, inc, 0);
        }

        private void minButton_Click(object sender, EventArgs e)
        {
            eth.print("axis: " + axis);
            eth.print("mov: " + movement);
            if (movement.Equals("Move"))
            {
                if (axis.Equals("X"))
                    cmd.IMOV(speed, -inc, 0, 0, 0, 0, 0, 0);
                else if (axis.Equals("Y"))
                    cmd.IMOV(speed, 0, -inc, 0, 0, 0, 0, 0);
                else
                    cmd.IMOV(speed, 0, 0, -inc, 0, 0, 0, 0);
            }
            else
                if (axis.Equals("X"))
                    cmd.IMOV(speed, 0, 0, 0, -inc, 0, 0, 0);
                else if (axis.Equals("Y"))
                    cmd.IMOV(speed, 0, 0, 0, 0, -inc, 0, 0);
                else
                    cmd.IMOV(speed, 0, 0, 0, 0, 0, -inc, 0);
        }

        private void alarmsButton_Click(object sender, EventArgs e)
        {
            cmd.RESET().ToString();
            alarmsButton.BackColor = Color.Transparent;
        }

        // select different robots
        private void rob1_Click(object sender, EventArgs e)
        {
            eth.print("Selected Robot 1");
            selectRobot(1);
        }

        private void rob2_Click(object sender, EventArgs e)
        {
            eth.print("Selected Robot 2");
            selectRobot(2);
        }

        // select specified robot, unless it is already selected
        // this function takes roughly 50ms to execute
        private void selectRobot(int rob)
        {
            if (rob == selectedRobot || (rob != 1 && rob != 2)) return; // if robot is already selected, just return and do nothing
            selectedRobot = rob;

            switch (rob)
            {
                case 1:
                    rob1.BackColor = Color.Green;
                    rob2.BackColor = Color.Transparent;
                    break;
                case 2:
                    rob1.BackColor = Color.Transparent;
                    rob2.BackColor = Color.Green;
                    break;
                default:
                    break;
            }
            curRobot = rob - 1;
            cmd.CGROUP(rob);
        }

        // tap watchdog timer to show that it is not needed
        // watchdog timer ensures that ethernet connection does not timeout by sending it routine commands
        private void watchdog_Tap()
        {
            watchdog.Stop();
            watchdog.Start();
        }

        bool mux = false;

        // ensures that ethernet connection does not time out
        private void watchdog_Tick(object sender, EventArgs e)
        {
            // alternate between checking posision and checking alarms
            if (!poll.Checked) return;

            if (mux)
                Posision_Click(null, null);
            else
            {
                bool alarm_found = false;
                string[] read = cmd.RALARM();
                alarmsText.Text = "";
                foreach (string s in read)
                {
                    try
                    {
                        if (int.Parse(s) != 0)
                            alarm_found = true;
                    }
                    catch (FormatException) { }
                    alarmsText.AppendText(s + "\n");

                    // decode alarms
                    try
                    {
                        int recInt = int.Parse(s);
                        switch (recInt)
                        {
                            case 4980:
                                eth.print("Alarm: Destination pulse limit");
                                break;
                            case 4684:
                                eth.print("Alarm: Interpolation invalid");
                                break;
                            case 4416:
                                eth.print("Alarm: Pulse limit");
                                break;
                            default:
                                break;
                        }
                    }
                    catch (FormatException) { }
                }
                if (alarm_found)
                    alarmsButton.BackColor = Color.Red;
                else
                    alarmsButton.BackColor = Color.Transparent;
            }
            mux = !mux;
        }

        // command line commands. not used
        private void commandLine_MouseDown(object sender, MouseEventArgs e)
        {
            if (commandLine.Text.Equals("Enter command here"))
                commandLine.Text = "";
        }

        // input commands directly into GUI
        // not really used
        private void commandLine_KeyPress(object sender, KeyPressEventArgs e)
        {
            if (e.KeyChar == '\r')
            {
                string[] cmdSplit = commandLine.Text.Split(' ');
                if (commandLine.Text.ToUpper().StartsWith("IMOV"))
                {
                    try
                    {
                        statusLabel.Text = "IMOV (" + cmdSplit[1] + " " + cmdSplit[2] + " " + cmdSplit[3] + " " + cmdSplit[4] + " " + cmdSplit[5] + " " + cmdSplit[6] + " " + cmdSplit[7] + ")";
                        cmd.IMOV(double.Parse(cmdSplit[1]), double.Parse(cmdSplit[2]), double.Parse(cmdSplit[3]), double.Parse(cmdSplit[4]), double.Parse(cmdSplit[5]), double.Parse(cmdSplit[6]), double.Parse(cmdSplit[7]), double.Parse(cmdSplit[8]));
                    }
                    catch (FormatException)
                    {
                        statusLabel.Text = "Incorrect format in command";
                    }
                }
                else if (commandLine.Text.ToUpper().StartsWith("MOVJ"))
                {
                    try
                    {
                        statusLabel.Text = "MOVJ (" + cmdSplit[1] + " " + cmdSplit[2] + " " + cmdSplit[3] + " " + cmdSplit[4] + " " + cmdSplit[5] + " " + cmdSplit[6] + " " + cmdSplit[7] + " " + cmdSplit[8] + ")";
                        cmd.MOVJ(double.Parse(cmdSplit[1]), double.Parse(cmdSplit[2]), double.Parse(cmdSplit[3]), double.Parse(cmdSplit[4]), double.Parse(cmdSplit[5]), double.Parse(cmdSplit[6]), double.Parse(cmdSplit[7]), double.Parse(cmdSplit[8]));
                    }
                    catch (IndexOutOfRangeException)
                    {
                        statusLabel.Text = "Incorrect number of arguments";
                    }
                    catch (FormatException)
                    {
                        statusLabel.Text = "Incorrect format in command";
                    }
                }
                else if (commandLine.Text.ToUpper().StartsWith("PMOVJ"))
                {
                    try
                    {
                        statusLabel.Text = "PMOVJ (" + cmdSplit[1] + " " + cmdSplit[2] + " " + cmdSplit[3] + " " + cmdSplit[4] + " " + cmdSplit[5] + " " + cmdSplit[6] + " " + cmdSplit[7] + " " + cmdSplit[8] + ")";
                        cmd.PMOVJ(double.Parse(cmdSplit[1]), int.Parse(cmdSplit[2]), int.Parse(cmdSplit[3]), int.Parse(cmdSplit[4]), int.Parse(cmdSplit[5]), int.Parse(cmdSplit[6]), int.Parse(cmdSplit[7]), int.Parse(cmdSplit[8]));
                    }
                    catch (IndexOutOfRangeException)
                    {
                        statusLabel.Text = "Incorrect number of arguments";
                    }
                    catch (FormatException)
                    {
                        statusLabel.Text = "Incorrect format in command";
                    }
                }
            }
            else
            {
                if (commandLine.Text.ToUpper().StartsWith("IMOV"))
                {
                    cmdLabel.Text = "Speed(mm/s)  X(mm)  Y(mm)  Z(mm)  Rx(deg)  Ry(deg)  Rz(deg)  Re(deg)";
                }
                else if (commandLine.Text.ToUpper().StartsWith("MOVJ"))
                {
                    cmdLabel.Text = "Speed(mm/s)  X(mm)  Y(mm)  Z(mm)  Rx(deg)  Ry(deg)  Rz(deg)  Re(deg)";
                }
                else if (commandLine.Text.ToUpper().StartsWith("PMOVJ"))
                {
                    cmdLabel.Text = "Speed(%)  S L U R B T E";
                }
            }
        }

        // gui buttons
        private void label1_Click(object sender, EventArgs e)
        {
        }

        // reset alarms
        private void resetButton_Click(object sender, EventArgs e)
        {
            int old_robot = curRobot;

            for (int a = 1; a <= 2; a++)
            {
                eth.print("RESET");
                selectRobot(a);
                eth.print(cmd.PMOVJ(35, 0, 0, 0, 0, 0, 0, 0).ToString());
                eth.print("DONE");
            }
            selectRobot(old_robot + 1);
        }

        // read current position for both robots
        private void Posision_Click(object sender, EventArgs e)
        {
            int old_robot = curRobot;
            for (int b = 1; b <= 2; b++)
            {
                selectRobot(b);
                double[] values = cmd.RPOSC();

                for (int a = 0; a < values.Length; a++)
                {
                    PosisionView.Items[a].SubItems[b].Text = values[a].ToString();
                }
            }
            selectRobot(old_robot + 1);
        }

        private void gotoPosision_Click(object sender, EventArgs e)
        {
        }

        // return both robots to home position
        private void homeButton_Click(object sender, EventArgs e)
        {
            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
            sw.Start();
            eth.print("");
            int old_robot = curRobot;

            for (int a = 1; a <= 2; a++)
            {
                SystemSounds.Beep.Play();
                eth.print("start:" + sw.ElapsedMilliseconds.ToString());
                selectRobot(a);
                SystemSounds.Beep.Play();
                eth.print("selected:" + sw.ElapsedMilliseconds.ToString());
                eth.print(cmd.PMOVJ(50, 0, 0, -100000, 0, 0, 0, 0).ToString());
                SystemSounds.Beep.Play();
                eth.print("moved:" + sw.ElapsedMilliseconds.ToString());
            }
            selectRobot(old_robot + 1);
        }

        private void Trackpad_Click(object sender, EventArgs e)
        {
            homeButton_Click(null, null);
            trackpad_en = !trackpad_en;
            if (trackpad_en)
            {
                Trackpad.BackColor = Color.Green;
                // move both robots to start position

                selectRobot(2);
                eth.print(cmd.MOVJ(50, 550, -350, 250, 90, 0, 90, 0).ToString());
                if (bothRobots)
                {
                    selectRobot(1);
                    eth.print(cmd.MOVJ(50, 550, 350, 250, 90, 0, 90, 0).ToString());
                }
            }
            else
                Trackpad.BackColor = Color.Maroon;
        }

        // pick up new movement from the trackpad and send new position to robot
        private void Trackpad_MouseMove(object sender, MouseEventArgs e)
        {
            if (trackpad_en)
            {
                if (bothRobots)
                    selectRobot(2);
                eth.print((-350 + (double)e.X / 5.0 - 50).ToString());
                watchdog_Tap();
                eth.print(cmd.MOVJ(50, 550, -350 + (double)e.X / 5.0 - 50, 250 - (e.Y / 5.0 - 50), 90, 0, 90, 0).ToString());
                if (bothRobots)
                {
                    selectRobot(1);
                    watchdog_Tap();
                    eth.print(cmd.MOVJ(50, 550, 350 - e.X / 5.0 - 50, 250 - (e.Y / 5.0 - 50), 90, 0, 90, 0).ToString());
                }
            }
        }

        bool hold_on = false;

        // toggle hold button
        public void hold(bool on)
        {
            hold_on = on;
            if (on)
            {
                cmd.HOLD(1);
                holdButton.BackColor = Color.Red;
            }
            else
            {
                cmd.HOLD(0);
                holdButton.BackColor = Color.Transparent;
            }
        }

        private void holdButton_Click(object sender, EventArgs e)
        {
            hold(!hold_on);
        }

        // random movement
        // not used
        private void button1_Click(object sender, EventArgs e)
        {
            int i = 50;
            int s = 100;
            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
            long oldTime = 0;

            oldTime = 0;
            sw.Reset();
            sw.Start();

            selectRobot(1);
            eth.print("S " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            cmd.IMOV(s, i, 0, 0, 0, 0, 0, 0);
            eth.print("M " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            selectRobot(2);
            eth.print("S " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            cmd.IMOV(s, i, 0, 0, 0, 0, 0, 0);
            eth.print("M " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            selectRobot(1);
            eth.print("S " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            cmd.IMOV(s, -i, 0, 0, 0, 0, 0, 0);
            eth.print("M " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            selectRobot(2);
            eth.print("S " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            cmd.IMOV(s, -i, 0, 0, 0, 0, 0, 0);
            eth.print("M " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
        }

        bool bothRobots = false;

        // toggle both robots to move from trackpad
        private void bothBox_CheckedChanged(object sender, EventArgs e)
        {
            bothRobots = bothBox.Checked;
        }

        private void startButton_Click(object sender, EventArgs e)
        {
            cmd.START();
        }

        // press button to move robot
        // numpad buttons control R2 and WASD control R1
        // 5 and S switch between rotate and move
        private void tabControl1_KeyPress(object sender, KeyPressEventArgs e)
        {
            if (!keypadBox.Checked) return; // dont respond to movement commands if this box is not checked
                
            double inc = 2.5;
            int speed = 150;
            switch (e.KeyChar)
            {
                // keyboard
                case 'z':
                    selectRobot(2);
                    if (move1)
                        cmd.IMOV(speed, 0, -inc, -inc, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, 0, inc, inc, 0);
                    break;
                case 'x':
                    selectRobot(2);
                    if (move1)
                        cmd.IMOV(speed, 0, 0, -inc, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, 0, inc, 0, 0);
                    break;
                case 'c':
                    selectRobot(2);
                    if (move1)
                        cmd.IMOV(speed, 0, inc, -inc, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, inc, 0, 0, inc, -inc, 0);
                    break;
                case 'a':
                    selectRobot(2);
                    if (move1)
                        cmd.IMOV(speed, 0, -inc, 0, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, 0, 0, inc, 0);
                    break;
                case 's':
                    selectRobot(2);
                    move1 = !move1;
                    if (move1)
                    {
                        eth.print("Switch to move");
                        inc = 2.5;
                    }
                    else
                    {
                        eth.print("Switch to rotate");
                        inc = 0.5;
                    }
                    break;
                case 'd':
                    selectRobot(2);
                    if (move1)
                        cmd.IMOV(speed, 0, inc, 0, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, inc, 0, 0, 0, -inc, 0);
                    break;
                case 'q':
                    selectRobot(2);
                    if (move1)
                        cmd.IMOV(speed, 0, -inc, inc, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, 0, -inc, inc, 0);
                    break;
                case 'w':
                    selectRobot(2);
                    if (move1)
                        cmd.IMOV(speed, 0, 0, inc, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, 0, -inc, 0, 0);
                    break;
                case 'e':
                    selectRobot(2);
                    if (move1)
                        cmd.IMOV(speed, 0, inc, inc, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, 0, -inc, -inc, 0);
                    break;
                case 'f':
                    selectRobot(2);
                    if (move1)
                        cmd.IMOV(speed, inc, 0, 0, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, inc, 0, 0, 0);
                    break;
                case 'r':
                    selectRobot(2);
                    if (move1)
                        cmd.IMOV(speed, -inc, 0, 0, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, -inc, 0, 0, 0);
                    break;
                case '.':
                    alarmsButton_Click(null, null);
                    break;

                // numpad
                case '1':
                    selectRobot(1);
                    if (move)
                        cmd.IMOV(speed, 0, -inc, -inc, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, 0, inc, inc, 0);
                    break;
                case '2':
                    selectRobot(1);
                    if (move)
                        cmd.IMOV(speed, 0, 0, -inc, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, 0, inc, 0, 0);
                    break;
                case '3':
                    selectRobot(1);
                    if (move)
                        cmd.IMOV(speed, 0, inc, -inc, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, inc, 0, 0, inc, -inc, 0);
                    break;
                case '4':
                    selectRobot(1);
                    if (move)
                        cmd.IMOV(speed, 0, -inc, 0, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, 0, 0, inc, 0);
                    break;
                case '5':
                    selectRobot(1);
                    move = !move;
                    if (move)
                    {
                        eth.print("Switch to move");
                        inc = 2.5;
                    }
                    else
                    {
                        eth.print("Switch to rotate");
                        inc = 0.5;
                    }
                    break;
                case '6':
                    selectRobot(1);
                    if (move)
                        cmd.IMOV(speed, 0, inc, 0, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, inc, 0, 0, 0, -inc, 0);
                    break;
                case '7':
                    selectRobot(1);
                    if (move)
                        cmd.IMOV(speed, 0, -inc, inc, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, 0, -inc, inc, 0);
                    break;
                case '8':
                    selectRobot(1);
                    if (move)
                        cmd.IMOV(speed, 0, 0, inc, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, 0, -inc, 0, 0);
                    break;
                case '9':
                    selectRobot(1);
                    if (move)
                        cmd.IMOV(speed, 0, inc, inc, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, 0, -inc, -inc, 0);
                    break;
                case '+':
                    selectRobot(1);
                    if (move)
                        cmd.IMOV(speed, inc, 0, 0, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, inc, 0, 0, 0);
                    break;
                case '-':
                    selectRobot(1);
                    if (move)
                        cmd.IMOV(speed, -inc, 0, 0, 0, 0, 0, 0);
                    else
                        cmd.IMOV(speed, 0, 0, 0, -inc, 0, 0, 0);
                    break;
                default:
                    break;
            }
        }

        private void num9_Click(object sender, EventArgs e)
        {
        }

        // movement button
        private void button3_Click(object sender, EventArgs e)
        {
            int speed = 1000;
            homeButton_Click(null, null);
            for (int x = 0; x < 10; x++)
            {
                selectRobot(1);
                cmd.IMOV(speed, 0, 0, 10, 0, 0, 0, 0);
                selectRobot(2);
                cmd.IMOV(speed, 0, 0, 10, 0, 0, 0, 0);
            }
            for (int x = 0; x < 10; x++)
            {
                selectRobot(1);
                cmd.IMOV(speed, 0, 0, -10, 0, 0, 0, 0);
                selectRobot(2);
                cmd.IMOV(speed, 0, 0, -10, 0, 0, 0, 0);
            }
        }

        // movement button
        private void button4_Click(object sender, EventArgs e)
        {
            int speed = 1000;
            double inc = 2.5;
            homeButton_Click(null, null);
            for (int x = 0; x < 50; x++)
            {
                selectRobot(1);
                cmd.IMOV(speed, 0, 0, inc, 0, 0, 0, 0);
                selectRobot(2);
                cmd.IMOV(speed, 0, 0, inc, 0, 0, 0, 0);
            }
            for (int x = 0; x < 50; x++)
            {
                selectRobot(1);
                cmd.IMOV(speed, 0, 0, -inc, 0, 0, 0, 0);
                selectRobot(2);
                cmd.IMOV(speed, 0, 0, -inc, 0, 0, 0, 0);
            }
        }

        // read commands file
        private void cmdFileButton_Click(object sender, EventArgs e)
        {
            readCommandsFile("Commands.csv");
        }

        // do the selected commands file
        private void fileList_ItemActivate(object sender, EventArgs e)
        {
            string filename = fileList.SelectedItems[0].Text;
            readCommandsFile(filename);
        }

        private void MainForm_FormClosing(object sender, FormClosingEventArgs e)
        {
            ftSensors2.stop_reading();
            ftSensors1.stop_reading();

            ftUpdate.Enabled = false;
            ftUpdate.Stop();
        }

        /**Displays new F/T data using InvokeLater, so it's thread-safe
        *@param displayRDT - the NetFTRDTPacket containing the F/T data to
        *display
        */

        double[] ft1RealReading, ft2RealReading;
        bool newValues = false;

        public void updateFTData(NetFTRDTPacket displayRDT, int NUM_FT_AXES, double[] m_daftCountsPerUnit, int sensor_id)
        {
            try
            {
                if (sensor_id == 1)
                {
                    ftSensors1.setRDTPacket(displayRDT);
                    string str = "";
                    str += ("0x" + String.Format("%08x", (int)displayRDT.getStatus()) + "\n");
                    str += ("" + displayRDT.getRDTSequence() + "\n");
                    str += ("" + displayRDT.getFTSequence() + "\n");
                    int[] ftCountsReading = displayRDT.getFTArray();
                    ft1RealReading = new double[NUM_FT_AXES];
                    int i;
                    for (i = 0; i < NUM_FT_AXES; i++)
                    {
                        /*display the numbers*/
                        ft1RealReading[i] = ftCountsReading[i] / m_daftCountsPerUnit[i];
                        str += (ft1RealReading[i].ToString("#.000") + "\n");
                    }
                }
                else
                {
                    ftSensors2.setRDTPacket(displayRDT);
                    string str = "";
                    str += ("0x" + String.Format("%08x", (int)displayRDT.getStatus()) + "\n");
                    str += ("" + displayRDT.getRDTSequence() + "\n");
                    str += ("" + displayRDT.getFTSequence() + "\n");
                    int[] ftCountsReading = displayRDT.getFTArray();
                    ft2RealReading = new double[NUM_FT_AXES];
                    int i;
                    for (i = 0; i < NUM_FT_AXES; i++)
                    {
                        /*display the numbers*/
                        ft2RealReading[i] = ftCountsReading[i] / m_daftCountsPerUnit[i];
                        str += (ft2RealReading[i].ToString("#.000") + "\n");
                    }
                }
                newValues = true;
            }
            catch (System.ComponentModel.Win32Exception)
            {
                // ignore, since this only happens when program is closing
            }
        }

        // timer ticks that synchronizes data read from sensors
        // set timeout in designer
        // if new values has been received, update gui elements and relay movement commands

        private void FTupdate_Tick(object sender, EventArgs e)
        {
            // wait 50 ms at the end if not moving, otherwise the gui tries to update too quickly
            if (newValues)
            {
                ftActivity.Checked = !ftActivity.Checked;

                // show data other than ft readings
                status1.Text = ((int)ftSensors1.getRDTPacket().getStatus()).ToString();
                RDT1.Text = ftSensors1.getRDTPacket().getRDTSequence().ToString();
                FTseq1.Text = ftSensors1.getRDTPacket().getFTSequence().ToString();

                // show ft readings
                update_gui_elements(ft1RealReading[0] - bias1[0], Fx1, Fx1progressBar);
                update_gui_elements(ft1RealReading[1] - bias1[1], Fy1, Fy1progressBar);
                update_gui_elements(ft1RealReading[2] - bias1[2], Fz1, Fz1progressBar);
                update_gui_elements(ft1RealReading[3] - bias1[3], Tx1, Tx1progressBar);
                update_gui_elements(ft1RealReading[4] - bias1[4], Ty1, Ty1progressBar);
                update_gui_elements(ft1RealReading[5] - bias1[5], Tz1, Tz1progressBar);

                status2.Text = ((int)ftSensors2.getRDTPacket().getStatus()).ToString();
                RDT2.Text = ftSensors2.getRDTPacket().getRDTSequence().ToString();
                FTseq2.Text = ftSensors2.getRDTPacket().getFTSequence().ToString();

                update_gui_elements(ft2RealReading[0] - bias2[0], Fx2, Fx2progressBar);
                update_gui_elements(ft2RealReading[1] - bias2[1], Fy2, Fy2progressBar);
                update_gui_elements(ft2RealReading[2] - bias2[2], Fz2, Fz2progressBar);
                update_gui_elements(ft2RealReading[3] - bias2[3], Tx2, Tx2progressBar);
                update_gui_elements(ft2RealReading[4] - bias2[4], Ty2, Ty2progressBar);
                update_gui_elements(ft2RealReading[5] - bias2[5], Tz2, Tz2progressBar);

                newValues = false;

                if (UDPtoggle.Checked)
                {
                    // move robots based on ft data
                    RobotControl.BiasFT(bias1, 1);
                    RobotControl.BiasFT(bias2, 2);
                    FT cmdFT = RobotControl.Inc_Control_Robot();

                    cmdFT.RoundFT();
                    if (cmdFT.time == -1) // no movement
                    {
                        Thread.Sleep(50);
                        return;
                    }
                    string str = cmdFT.Fx + " " + cmdFT.Fy + " " + cmdFT.Fz + " " + cmdFT.Tx + " " + cmdFT.Ty + " " + cmdFT.Tz + " ";
                    outputBox.AppendText(str + "\n");
                    inputString("1,IMOV," + (5 * largest(cmdFT)) + "," + cmdFT.Fz + "," + cmdFT.Fx + "," + cmdFT.Fy + "," + cmdFT.Tz + "," + cmdFT.Tx + "," + cmdFT.Ty + ",0");
                    return;
                }
                Thread.Sleep(50);
            }
        }

        // simple algorithm to calculate largest movement
        private double largest(FT ftin)
        {
            double large = Math.Max(Math.Abs(ftin.Fx), Math.Abs(ftin.Fy));
            large = Math.Max(Math.Abs(ftin.Fz), large);
            large = Math.Max(Math.Abs(ftin.Tx), large);
            large = Math.Max(Math.Abs(ftin.Ty), large);
            large = Math.Max(Math.Abs(ftin.Tz), large);
            return large;
        }

        // update GUI elements with data
        private Color POSITIVE_COLOR = Color.Red, NEGATIVE_COLOR = Color.Lime;

        // just for drawing progress bars
        int max_force = 10000;
        int max_torque = 1000;
        private void update_gui_elements(double dataIn, TextBox tb, VistaStyleProgressBar.ProgressBar pb)
        {
            tb.Text = String.Format("{0:0.00}", dataIn);
            if (dataIn < 0)
            {
                pb.StartColor = NEGATIVE_COLOR;
                pb.EndColor = NEGATIVE_COLOR;
            }
            else
            {
                pb.StartColor = POSITIVE_COLOR;
                pb.EndColor = POSITIVE_COLOR;
            }


            // check that value falls in limits:
            int newVal = (int)(Math.Abs(dataIn) * 1000);
            if (pb.Tag.Equals("Force"))
            {
                if (newVal > max_force)
                    max_force = newVal;
                pb.MaxValue = max_force;
            }
            else if (pb.Tag.Equals("Torque"))
            {
                if (newVal > max_torque)
                    max_torque = newVal;
                pb.MaxValue = max_torque;
            }
            pb.Value = newVal;
        }

        internal bool IsSending()
        {
            return UDPtoggle.Checked;
        }

        // bias values for sensors 1 and 2
        double[] bias1 = new double[6];
        double[] bias2 = new double[6];
        // bias sensors so current position is considered "zero ft"
        private void biasButton_Click(object sender, EventArgs e)
        {
            Array.Copy(ft1RealReading, bias1, ft1RealReading.Length);
            Array.Copy(ft2RealReading, bias2, ft2RealReading.Length);
        }

        public void UpdateRobot(NetFTRDTPacket RDT, int sensor_id)
        {
            if (sensor_id == 1)
                RobotControl.UpdateSensor1(RDT);
            else if (sensor_id == 2)
                RobotControl.UpdateSensor2(RDT);
        }

    }
}